/********************************** (C) COPYRIGHT *******************************
* File Name          : foc.c
* Author             : nx666
* Version            : V1.0.0
* Date               : 2023/02/27
* Description        : foc主要接口这里实现接口.
*******************************************************************************/
/***********************************头文件区域***********************************/
#include "IQmathLib.h"
#include "ch32f20x_opa.h"
#include "ch32f20x_usart.h"
#include "fiter.h"
#include "module_config.h"
#include "string.h"
#include "driver.h"
#include "common_type.h"
#include "FreeRTOS.h"
#include "semphr.h"
#include "foc.h"
#include "malloc.h"
#include "interact.h"


/***********************************宏定义区***********************************/


/******************************** *全局变量区**********************************/

/*********************************************************************
 * @fn      getFreq
 *
 * @brief   计算a相电流信号的频率，
 *              void
 *
 * @return  0，成功  -1，失败
 *          
 */
static _iq getFreq(struct sampling * pSamp)
{

    static uInt32 count = 0;
    static _iq  Ia_last = -1;
    _iq Ia = 0;

    Ia = pSamp->Ia;
    count++;
    pSamp->theta += _IQ8(1);
    if (Ia_last <  0 && (Ia > 0))
    {
        //min(count, FOC_CALC_CYCLE);没考虑转速慢的情况
        pSamp->realf = count < FOC_CALC_CYCLE ?  _IQ8(count) : _IQ8(FOC_CALC_CYCLE);
        count = 0;
        pSamp->theta = 0;
    }
    // printMat[0] = (float)(Ia);
    // printMat[1] = (float)(k);
    if (!(Ia == 0))
    {
        Ia_last = Ia;
    }
    else 
    {
        Ia_last = -1;
    }
    if (count > 16777216) //1<<24
    {
        pSamp->f = 0;
    }

    //用于计算幅度值的f
    pSamp->f = _IQ8(FOC_CALC_CYCLE);

    return pSamp->realf;
}

/*********************************************************************
 * @fn      adjustWaveform
 *
 * @brief   调整波形，
 *              void
 *
 * @return  0，成功  -1，失败
 *          
 */
static Int adjustWaveform(struct sampling * pSamp)
{
    _iq aAdjst;
    _iq bAdjst;
    _iq cAdjst;

    //垂直方向调整到零点
    aAdjst = (pSamp->Ia_lower + pSamp->Ia_upper) >> 1;
    pSamp->Ia -= aAdjst;
    bAdjst = (pSamp->Ib_lower + pSamp->Ib_upper) >> 1;
    pSamp->Ib -= bAdjst;
    cAdjst = (pSamp->Ic_lower + pSamp->Ic_upper) >> 1;
    pSamp->Ic -= cAdjst;

    //垂直方向缩放
    pSamp->Ia = _IQ8mpy(pSamp->Ia, pSamp->Ia_scalingFactor);
    pSamp->Ib = _IQ8mpy(pSamp->Ib, pSamp->Ib_scalingFactor);
    pSamp->Ic = _IQ8mpy(pSamp->Ic, pSamp->Ic_scalingFactor);
    return 0;
}

/*********************************************************************
 * @fn      getEnvelope
 *
 * @brief   计算电流的包络线，非重入函数
 *              void
 *
 * @return  0，成功  -1，失败
 *          
 */
static Int getEnvelope(struct sampling * pSamp)
{
    static uInt32 count=0;
    static _iq        Ia_upper = 0;
    static _iq        Ia_lower = 0;
    static _iq        Ib_upper = 0;
    static _iq        Ib_lower = 0;
    static _iq        Ic_upper = 0;
    static _iq        Ic_lower = 0;
    _iq               aPeak;
    _iq               bPeak;
    _iq               cPeak;
    _iq               peakAve;

    uInt32 freq = pSamp->f >> 8;

    //找到一个周期内的最大、最小值
    if (count++ < freq)
    {
        if (Ia_upper < pSamp->Ia)
        {
            Ia_upper = pSamp->Ia;
        }
        if (Ia_lower > pSamp->Ia)
        {
            Ia_lower = pSamp->Ia;
        }
        if (Ib_upper < pSamp->Ib)
        {
            Ib_upper = pSamp->Ib;
        }
        if (Ib_lower > pSamp->Ib)
        {
            Ib_lower = pSamp->Ib;
        }
        if (Ic_upper < pSamp->Ic)
        {
            Ic_upper = pSamp->Ic;
        }
        if (Ic_lower > pSamp->Ic)
        {
            Ic_lower = pSamp->Ic;
        }
    }
    else 
    {
        //更新包络线
        pSamp->Ia_lower = _IQ8mpy(Ia_lower,76) + _IQ8mpy(pSamp->Ia_lower, 180);  // 0.3
        pSamp->Ia_upper = _IQ8mpy(Ia_upper,76) + _IQ8mpy(pSamp->Ia_upper, 180);
        pSamp->Ib_lower = _IQ8mpy(Ib_lower,76) + _IQ8mpy(pSamp->Ib_lower, 180);  // 0.3
        pSamp->Ib_upper = _IQ8mpy(Ib_upper,76) + _IQ8mpy(pSamp->Ib_upper, 180);
        pSamp->Ic_lower = _IQ8mpy(Ic_lower,76) + _IQ8mpy(pSamp->Ic_lower, 180);  // 0.3
        pSamp->Ic_upper = _IQ8mpy(Ic_upper,76) + _IQ8mpy(pSamp->Ic_upper, 180);
        Ia_lower = 0x7FFFFFFF;
        Ia_upper = -0x7FFFFFFF;
        Ib_lower = 0x7FFFFFFF;
        Ib_upper = -0x7FFFFFFF;
        Ic_lower = 0x7FFFFFFF;
        Ic_upper = -0x7FFFFFFF;
        count = 1;

        //更新缩放系数
        aPeak = pSamp->Ia_upper - pSamp->Ia_lower;
        bPeak = pSamp->Ib_upper - pSamp->Ib_lower;
        cPeak = pSamp->Ic_upper - pSamp->Ic_lower;
        peakAve = _IQ8mpy((aPeak + bPeak + cPeak), 85);  // (aPeak + bPeak + cPeak)/3
        if (peakAve)
        {
            pSamp->Ia_scalingFactor = _IQ8div(peakAve, aPeak);
            pSamp->Ib_scalingFactor = _IQ8div(peakAve, bPeak);
            pSamp->Ic_scalingFactor = _IQ8div(peakAve, cPeak);
        }
    }
    return 0;
}

static Int createWave(struct sampling * pSamp) 
{
    _iq Aa,Ab,Ac;
    _iq pha,phb,phc;

    Aa = pSamp->Ia_upper - ((pSamp->Ia_upper + pSamp->Ia_lower) >> 1);
    Ab = pSamp->Ib_upper - ((pSamp->Ib_upper + pSamp->Ib_lower) >> 1);
    Ac = pSamp->Ic_upper - ((pSamp->Ic_upper + pSamp->Ic_lower) >> 1);

    Aa = _IQ8mpy(Aa, pSamp->Ia_scalingFactor);
    Ab = _IQ8mpy(Ab, pSamp->Ib_scalingFactor);
    Ac = _IQ8mpy(Ac, pSamp->Ic_scalingFactor);

    pha = _IQ8div(pSamp->theta,pSamp->realf);
    phb = (pha + 171) & (0xFF);
    phc = (pha + 85) & (0xFF);

    pSamp->Ia = _IQ8mpy(_IQ8sinPU(pha), Aa);
    pSamp->Ib = _IQ8mpy(_IQ8sinPU(phb), Ab);
    pSamp->Ic = _IQ8mpy(_IQ8sinPU(phc), Ac);

    return 0;
}

/*********************************************************************
 * @fn      doSampling
 *
 * @brief   完成一次电流采样，
 *              void
 *
 * @return  0，成功  -1，失败
 *          
 */
static Int doSampling(struct sampling * pSamp)
{

    pSamp->adc->getCurrentIaIbIc(pSamp->adc);

    pSamp->Ia_orig = _IQ8mpy(pSamp->adc->Ia, pSamp->lpf) + _IQ8mpy(pSamp->Ia_orig, pSamp->_lpf);
    pSamp->Ib_orig = _IQ8mpy(pSamp->adc->Ib, pSamp->lpf) + _IQ8mpy(pSamp->Ib_orig, pSamp->_lpf);
    pSamp->Ic_orig = _IQ8mpy(pSamp->adc->Ic, pSamp->lpf) + _IQ8mpy(pSamp->Ic_orig, pSamp->_lpf);

    pSamp->Ia_orig = pSamp->fiterA->update(pSamp->fiterA, pSamp->Ia_orig);
    pSamp->Ib_orig = pSamp->fiterB->update(pSamp->fiterB, pSamp->Ib_orig);
    pSamp->Ic_orig = pSamp->fiterC->update(pSamp->fiterC, pSamp->Ic_orig);

    pSamp->Ia = pSamp->Ia_orig;
    pSamp->Ib = pSamp->Ib_orig;
    pSamp->Ic = pSamp->Ic_orig;
    
    getEnvelope(pSamp);
    adjustWaveform(pSamp);
    getFreq(pSamp);

    //createWave(pSamp);

    // printMat[0] = (float)(pSamp->Ia_orig);
    // printMat[1] = (float)(pSamp->Ib_orig);
    // printMat[2] = (float)(pSamp->Ic_orig);

    // printMat[0] = (float)(pSamp->adc->Ic);
    // printMat[1] = (float)(pSamp->Ic_upper);
    // printMat[2] = (float)(pSamp->Ic_lower);
    // printMat[1] = (float)(pSamp->adc->Ib);
    // printMat[2] = (float)(pSamp->adc->Ia);

    // printData(printMat,3);

    return 0;
}


/*********************************************************************
 * @fn      createSampling
 *
 * @brief   采样模块构造函数，
 *              void
 *
 * @return  sampling_ *，成功  NULL，失败
 *          
 */
sampling_ *createSampling(void)
{
    sampling_ *priv;

    priv = (sampling_ *)malloc(sizeof(sampling_));
    if (!priv)
    {
        return NULL;
    }
    memset((void *)priv, 0, sizeof(sampling_));

    priv->adc = createAdc();
    priv->fiterA = create_movingAverage(FOC_SAMPLING_FITER_WINDOW_SIZE);
    priv->fiterB = create_movingAverage(FOC_SAMPLING_FITER_WINDOW_SIZE);
    priv->fiterC = create_movingAverage(FOC_SAMPLING_FITER_WINDOW_SIZE);
    priv->lpf = _IQ8(FOC_SAMPLING_LPF_K);
    priv->_lpf = _IQ8(1 - FOC_SAMPLING_LPF_K);
    priv->f = _IQ8(FOC_CALC_CYCLE);
    priv->doSampling = doSampling;

    return priv;
}








